using BepuPhysics.Collidables;
using BepuUtilities;
using BepuUtilities.Memory;
using System;
using System.Diagnostics;
using System.Runtime.CompilerServices;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif

namespace BepuPhysics.CollisionDetection
{
    public struct MeshReduction : ICollisionTestContinuation
    {
        /// <summary>
        /// 用于将接触标记为由其特征ID中的三角形的面生成的标志。
        /// </summary>
        public const int FaceCollisionFlag = 32768;
        /// <summary>
        /// 将碰撞视为三角面接触的三角面与接触法线之间的最小点积。
        /// </summary>
        public const float MinimumDotForFaceCollision = 0.999999f;
        public Buffer<Triangle> Triangles;
        // 网格还原依赖于网格的所有三角形都在窗口B中,因为它们显示在网格碰撞任务中。
        // 然而,原始用户可能以未知的顺序提供了这一对,并触发了翻转。我们会在检查接触位置时补偿这一点。
        public bool RequiresFlip;
        // 三角形数组位于网格的局部空间中。为了针对它们测试任何联系人,我们需要能够转换联系人。
        public Quaternion MeshOrientation;
        public BoundingBox QueryBounds;
        // 这使用了所有非凸约简的逻辑,所以我们只需嵌套它。
        public NonconvexReduction Inner;

        public void Create(int childManifoldCount, BufferPool pool)
        {
            Inner.Create(childManifoldCount, pool);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public unsafe void OnChildCompleted<TCallbacks>(ref PairContinuation report, ref ConvexContactManifold manifold, ref CollisionBatcher<TCallbacks> batcher)
            where TCallbacks : struct, ICollisionCallbacks
        {
            Inner.OnChildCompleted(ref report, ref manifold, ref batcher);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void OnChildCompletedEmpty<TCallbacks>(ref PairContinuation report, ref CollisionBatcher<TCallbacks> batcher) where TCallbacks : struct, ICollisionCallbacks
        {
            Inner.OnChildCompletedEmpty(ref report, ref batcher);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static unsafe void ComputeMeshSpaceContacts(ref ConvexContactManifold manifold, in Matrix3x3 inverseMeshOrientation, bool requiresFlip, Vector3* meshSpaceContacts, out Vector3 meshSpaceNormal)
        {
            // 首先,如果流形认为网格及其三角形是B形状,那么我们需要将其翻转。
            if (requiresFlip)
            {
                // 如果流形将网格及其三角形视为形状B,则需要在变换之前将其翻转。
                for (int i = 0; i < manifold.Count; ++i)
                {
                    Matrix3x3.Transform(Unsafe.Add(ref manifold.Contact0, i).Offset - manifold.OffsetB, inverseMeshOrientation, out meshSpaceContacts[i]);
                }
                Matrix3x3.Transform(-manifold.Normal, inverseMeshOrientation, out meshSpaceNormal);
            }
            else
            {
                // 不需要翻转。
                for (int i = 0; i < manifold.Count; ++i)
                {
                    Matrix3x3.Transform(Unsafe.Add(ref manifold.Contact0, i).Offset, inverseMeshOrientation, out meshSpaceContacts[i]);
                }
                Matrix3x3.Transform(manifold.Normal, inverseMeshOrientation, out meshSpaceNormal);
            }
        }

        struct TestTriangle
        {
            // 测试三角形包含AOS化布局,以便更快地按触点进行测试。
            public Vector4 AnchorX;
            public Vector4 AnchorY;
            public Vector4 AnchorZ;
            public Vector4 NX;
            public Vector4 NY;
            public Vector4 NZ;
            public float DistanceThreshold;
            public int ChildIndex;
            /// <summary>
            /// 如果与此三角形关联的流形由于检测到对另一个三角形的侵犯而被阻塞,则为True,否则为False。
            /// </summary>
            public bool Blocked;
            /// <summary>
            /// 如果三角形没有充当任何其他流形的阻塞器,则为True,因此如果它被阻挡,则可以删除;否则为False。
            /// </summary>
            public bool ForceDeletionOnBlock;
            /// <summary>
            /// 检测到被网格空间中与此三角形关联的流形侵犯的三角形的法线。
            /// </summary>
            public Vector3 CorrectedNormal;

            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public TestTriangle(in Triangle triangle, int sourceChildIndex)
            {
                var ab = triangle.B - triangle.A;
                var bc = triangle.C - triangle.B;
                var ca = triangle.A - triangle.C;
                // TODO：在处理小三角形时,此阈值可能会导致凹凸。可能希望包含不同的比例信息源,如原始凸面测试。
                DistanceThreshold = 1e-3f * (float)Math.Sqrt(MathHelper.Max(triangle.A.LengthSquared() * 1e-4f, MathHelper.Max(ab.LengthSquared(), ca.LengthSquared())));
                var n = Vector3.Cross(ab, ca);
                // 边法线指向外部。
                var edgeNormalAB = Vector3.Cross(n, ab);
                var edgeNormalBC = Vector3.Cross(n, bc);
                var edgeNormalCA = Vector3.Cross(n, ca);

                NX = new Vector4(n.X, edgeNormalAB.X, edgeNormalBC.X, edgeNormalCA.X);
                NY = new Vector4(n.Y, edgeNormalAB.Y, edgeNormalBC.Y, edgeNormalCA.Y);
                NZ = new Vector4(n.Z, edgeNormalAB.Z, edgeNormalBC.Z, edgeNormalCA.Z);
                var normalLengthSquared = NX * NX + NY * NY + NZ * NZ;
                var inverseLength = Vector4.One / Vector4.SquareRoot(normalLengthSquared);
                NX *= inverseLength;
                NY *= inverseLength;
                NZ *= inverseLength;
                AnchorX = new Vector4(triangle.A.X, triangle.A.X, triangle.B.X, triangle.C.X);
                AnchorY = new Vector4(triangle.A.Y, triangle.A.Y, triangle.B.Y, triangle.C.Y);
                AnchorZ = new Vector4(triangle.A.Z, triangle.A.Z, triangle.B.Z, triangle.C.Z);

                ChildIndex = sourceChildIndex;
                Blocked = false;
                ForceDeletionOnBlock = true;
                CorrectedNormal = default;
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static unsafe bool ShouldBlockNormal(in TestTriangle triangle, in Vector3 meshSpaceContact, in Vector3 meshSpaceNormal)
        {
            // 虽然我们没有一种合适的方法在单个流形与三角形测试的上下文中进行真正的SIMD操作缩放,但我们至少可以使用4宽的操作
            // 以加速每个单独的接触测试。
            // DistanceFromPlane=(Position-a)*N/||N||
            // 距平面的距离^2=((位置-a)*N)^2/(N*N)
            // DistanceAlongEdgeNormal^2=((Position-EdgeStart)*Edgen)^2/||Edgen||^2

            // 有四条车道,每个考虑平面一条：
            // X：平面法线
            // Y：AB边法线
            // Z：BC边法线
            // W：CA边缘法线
            // 它们都是一样的手术,所以我们可以做4倍宽的手术。这比做一堆单独的水平点产品要好。
            var px = new Vector4(meshSpaceContact.X);
            var py = new Vector4(meshSpaceContact.Y);
            var pz = new Vector4(meshSpaceContact.Z);
            var offsetX = px - triangle.AnchorX;
            var offsetY = py - triangle.AnchorY;
            var offsetZ = pz - triangle.AnchorZ;
            var distanceAlongNormal = offsetX * triangle.NX + offsetY * triangle.NY + offsetZ * triangle.NZ;
            // 请注意,非常细的三角形可能会因为不检查真实距离而导致接受有问题-
            // 位置可能远远超出顶点,但仍在边平面阈值内。我们假设这个问题的影响将是微乎其微的。
            if (distanceAlongNormal.X <= triangle.DistanceThreshold &&
                distanceAlongNormal.Y <= triangle.DistanceThreshold &&
                distanceAlongNormal.Z <= triangle.DistanceThreshold &&
                distanceAlongNormal.W <= triangle.DistanceThreshold)
            {
                // 触点在三角形附近。正常是否侵犯了三角形的面区域？
                // 在以下情况下会发生这种情况：
                // 1)接触位于一条边附近,法线沿该边向内指向法线。
                // 2)触点在三角形的内侧。
                // 请注意,我们对处于边缘比在附近更严格。
                // 这是因为侵权检查要求沿着触点所在的每一条边都有正常的侵权行为;
                // 过于激进的边缘分类有时会导致侵权行为被忽视。
                var negativeThreshold = triangle.DistanceThreshold * -1e-2f;
                var onAB = distanceAlongNormal.Y >= negativeThreshold;
                var onBC = distanceAlongNormal.Z >= negativeThreshold;
                var onCA = distanceAlongNormal.W >= negativeThreshold;
                if (!onAB && !onBC && !onCA)
                {
                    // 触点在三角形内。
                    // 如果此接触导致更正,我们可以跳过此歧管中的其余接触。
                    return true;
                }
                else
                {
                    // 触点位于三角形的边界上。法线是否在触点所在的任何边向外指向？
                    // 请记住,接触已推入网格空间。位置在三角形的曲面上,法线点从凸面到网格面。
                    // 边平面法线从三角形指向外部,因此如果检测到接触法线指向边平面法线,
                    // 那就是侵权了。
                    var normalDot = triangle.NX * meshSpaceNormal.X + triangle.NY * meshSpaceNormal.Y + triangle.NZ * meshSpaceNormal.Z;
                    const float infringementEpsilon = 1e-6f;
                    // 为了挡路一个联系人,它必须在它所在的每一条边缘上都受到侵犯。
                    // 换句话说,当接触位于顶点上时,仅侵犯其中一条边是不够的;在这种情况下,接触法线不是
                    // 实际上侵犯了三角面。
                    // 此外,请注意,我们要求非零正侵权;否则,我们最终会阻止平面邻居的联系。
                    // 但我们在阻止"第二次"边缘侵犯方面更具攻击性--如果它仅仅是平行的,我们就把它算作侵权行为。
                    // 否则,您可能会遇到这样的情况,即一组不同三角形的顶点上的接触不会被其中任何一个阻挡,因为
                    // 法线上有一条边。
                    if ((onAB && normalDot.Y > infringementEpsilon) || (onBC && normalDot.Z > infringementEpsilon) || (onCA && normalDot.W > infringementEpsilon))
                    {
                        const float secondaryInfringementEpsilon = -1e-3f;
                        // 至少有一条边被侵犯。所有接触过的边缘都至少几乎被侵犯了吗？
                        if ((!onAB || normalDot.Y > secondaryInfringementEpsilon) && (!onBC || normalDot.Z > secondaryInfringementEpsilon) && (!onCA || normalDot.W > secondaryInfringementEpsilon))
                        {
                            return true;
                        }
                    }
                }
            }
            return false;
        }

        public unsafe static void ReduceManifolds(ref Buffer<Triangle> continuationTriangles, ref Buffer<NonconvexReductionChild> continuationChildren, int start, int count,
           bool requiresFlip, in BoundingBox queryBounds, in Matrix3x3 meshOrientation, in Matrix3x3 meshInverseOrientation, BufferPool pool)
        {
            // 在将责任移交给非凸化简之前,请确保没有任何接触在三角形边界造成严重的"凹凸"。
            // 当孤立的三角形测试检测到指向外部的触点时,可能会发生凸起,例如当盒子撞到侧面时。当三角形真的是孤立的时候,这是很好的,
            // 但是,如果有一个相邻的三角形是紧密相连的,用户可能希望这两个三角形的行为像一个连贯的曲面。所以,联系人
            // 如果法线不存在于理想的"连续"形式的曲面中,则需要进行校正。

            // 如果联系人满足以下三个条件,则该联系人是需要更正的候选联系人：
            // 1)接触不是由面碰撞产生的,并且
            // 2)接触位置与另一个三角形接触,
            // 3)接触法线侵犯邻居脸部Voronoi区。

            // 面碰撞生成的接触始终立即接受,无需修改。
            // 只有当表面凹陷时,它们才能引起侵权,在这种情况下,面部法线是正确的,不会造成任何不适当的凹凸。

            // 没有接触三角形的触点不能侵犯它。
            // 请注意,涉及三角形的流形始终会生成接触,以便位置位于三角形上,从而使此测试有意义。
            // (这就是为什么网格简化必须知道流形是否已翻转-以便我们知道我们使用的是一致的插槽。)

            // 由面碰撞产生的接触用特殊的特征ID标志来标记。如果存在,我们可以跳过联系。碰撞测试器还提供了唯一的功能ID
            // 在那面旗子后面,所以我们现在可以剥旗子了。(我们实际上只是劫持了功能ID来存储一些临时元数据。)

            // TODO：请注意,我们在减少之前执行接触校正。约简依赖于法线来计算其"显著性"启发式。
            // 您可以牺牲一点减量质量来实现更快的接触校正(因为减量会产生较低的固定接触数),但是
            // 只有当联系方式更正是一项有意义的成本时,我们才应该追求这一点。

            // 缩小感兴趣区域。
            continuationTriangles.Slice(start, count, out var triangles);
            continuationChildren.Slice(start, count, out var children);
            // 为所有可能的三角形分配足够的空间,即使我们将只枚举实际具有联系人的子集。
            int activeChildCount = 0;
            Buffer<TestTriangle> activeTriangles;
            // 在病理情况下避免堆栈爆炸。他们会发生的！
            const int heapAllocateThreshold = 1024;
            if (count > heapAllocateThreshold)
            {
                pool.Take(count, out activeTriangles);
            }
            else
            {
                var memory = stackalloc TestTriangle[count];
                activeTriangles = new Buffer<TestTriangle>(memory, count);
            }
            for (int i = 0; i < count; ++i)
            {
                if (children[i].Manifold.Count > 0)
                {
                    activeTriangles[activeChildCount] = new TestTriangle(triangles[i], i);
                    ++activeChildCount;
                }
            }
            var meshSpaceContacts = stackalloc Vector3[4];
            for (int i = 0; i < activeChildCount; ++i)
            {
                ref var sourceTriangle = ref activeTriangles[i];
                ref var sourceChild = ref children[sourceTriangle.ChildIndex];
                // 无法更正面碰撞创建的联系人。
                if ((sourceChild.Manifold.Contact0.FeatureId & FaceCollisionFlag) == 0)
                {
                    ComputeMeshSpaceContacts(ref sourceChild.Manifold, meshInverseOrientation, requiresFlip, meshSpaceContacts, out var meshSpaceNormal);
                    // 从歧管中选择最深的触点。我们的目标是在源三角形的代表性特征上找到联系人。
                    // 回想一下,三角形碰撞测试将在三角形的其他位置生成推测性接触,既在面上,也可能在边上
                    // 除了最深的边缘。
                    // 然而,"正常"与最深层次的接触最为直接相关。事实上,普通人在其他方面"侵犯"了自己的利益,这一点并不重要。
                    // (为什么这无关紧要？网格还原一次在单个凸面网格对上运行。"凸形"形状不能同时在三角形的两侧产生真正的侵权接触。
                    // 相对边的接触实际上会从那条边指向"远离"源三角形的内部。出于同样的原因,我们从来不会面对挡路联系人,这是没有意义的
                    // (挡路基于这些偶然接触。)
                    // 这等同于使用法线来确定流形Voronoi区域,只是接触位置让我们可以处理更多的任意内容。
                    var deepestIndex = 0;
                    var deepestDepth = sourceChild.Manifold.Contact0.Depth;
                    for (int j = 1; j < sourceChild.Manifold.Count; ++j)
                    {
                        var depth = Unsafe.Add(ref sourceChild.Manifold.Contact0, j).Depth;
                        if (deepestDepth < depth)
                        {
                            deepestDepth = depth;
                            deepestIndex = j;
                        }
                    }
                    ref var meshSpaceContact = ref meshSpaceContacts[deepestIndex];
                    for (int j = 0; j < activeChildCount; ++j)
                    {
                        // 试图对照其自身的三角形来检查法线是没有意义的。
                        if (i != j)
                        {
                            ref var targetTriangle = ref activeTriangles[j];
                            if (ShouldBlockNormal(targetTriangle, meshSpaceContact, meshSpaceNormal))
                            {
                                sourceTriangle.Blocked = true;
                                sourceTriangle.CorrectedNormal = new Vector3(targetTriangle.NX.X, targetTriangle.NY.X, targetTriangle.NZ.X);
                                // 即使目标流形被阻塞,也不一定要将其删除。我们把它当做拦网用的。
                                targetTriangle.ForceDeletionOnBlock = false;
                                break;
                            }
                        }
                    }
                    // 请注意,删除必须推迟到阻塞分析之后。
                    // 此循环的其余部分将不考虑此流形,因此可以对其进行修改。
                    for (int j = sourceChild.Manifold.Count - 1; j >= 0; --j)
                    {
                        // 如果接触位于找到要测试的三角形的网格空间边界框之外,则有两点是正确的：
                        // 1)接触几乎肯定是无效的;包围盒包括一个集成运动的框架,而该接触在其外部。
                        // 2)联系人可能是用邻居不在查询范围内的三角形创建的,因此邻居不会造成任何阻塞。
                        // 其结果是,这样的接触有导致幽灵碰撞的趋势。我们宁愿不强制使用非常小的投机保证金,
                        // 因此,相反,我们显式删除查询边界之外的联系人。
                        ref var contactToCheck = ref meshSpaceContacts[j];
                        if (Vector3.Min(contactToCheck, queryBounds.Min) != queryBounds.Min ||
                            Vector3.Max(contactToCheck, queryBounds.Max) != queryBounds.Max)
                        {
                            ConvexContactManifold.FastRemoveAt(ref sourceChild.Manifold, j);
                        }
                    }
                }
                else
                {
                    // 把脸上的旗子清理干净。这不是"必需的",因为它们无论如何都足够连贯,而且累积的脉冲再分配器是一种不错的后备方案,
                    // 但这样做基本上是零成本的。
                    for (int k = 0; k < sourceChild.Manifold.Count; ++k)
                    {
                        Unsafe.Add(ref sourceChild.Manifold.Contact0, k).FeatureId &= ~FaceCollisionFlag;
                    }
                }
            }
            for (int i = 0; i < activeChildCount; ++i)
            {
                ref var triangle = ref activeTriangles[i];
                if (triangle.Blocked)
                {
                    ref var manifold = ref children[triangle.ChildIndex].Manifold;
                    if (triangle.ForceDeletionOnBlock)
                    {
                        // 这个歧管是侵权的,没有其他歧管侵犯它。可以安全地完全忽略歧管。
                        manifold.Count = 0;
                    }
                    else
                    {
                        var manifoldHasPositiveDepth = false;
                        for (int j = 0; j < manifold.Count; ++j)
                        {
                            if (Unsafe.Add(ref manifold.Contact0, j).Depth > 0)
                            {
                                manifoldHasPositiveDepth = true;
                                break;
                            }
                        }
                        if (manifoldHasPositiveDepth)
                        {
                            // 这个歧管正在侵权,但另一个歧管正在侵权。我们不能安全地删除这样的流形,因为它很可能是相互侵权的
                            // 大小写-考虑当对象将自身楔入两个三角形之间的边时会发生什么情况。
                            Matrix3x3.Transform(requiresFlip ? triangle.CorrectedNormal : -triangle.CorrectedNormal, meshOrientation, out manifold.Normal);
                            // 请注意,我们不修改深度。
                            // 唯一应该发生这种情况的情况是当对象以某种方式夹在相邻三角形之间时,检测到的
                            // 深度比三角面的深度"小"。因此,使用这些深度可以保证不会引入过多的能量。
                        }
                        else
                        {
                            // 流形的深度为零或负;这显然不是一个形状夹在三角形之间的情况。把它扔掉就行了。
                            manifold.Count = 0;
                        }


                    }
                }
            }
            if (count > heapAllocateThreshold)
            {
                pool.Return(ref activeTriangles);
            }
        }

        // [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public unsafe bool TryFlush<TCallbacks>(int pairId, ref CollisionBatcher<TCallbacks> batcher) where TCallbacks : struct, ICollisionCallbacks
        {
            Debug.Assert(Inner.ChildCount > 0);
            if (Inner.CompletedChildCount == Inner.ChildCount)
            {
                Matrix3x3.CreateFromQuaternion(MeshOrientation, out var meshOrientation);
                Matrix3x3.Transpose(meshOrientation, out var meshInverseOrientation);

                ReduceManifolds(ref Triangles, ref Inner.Children, 0, Inner.ChildCount, RequiresFlip, QueryBounds, meshOrientation, meshInverseOrientation, batcher.Pool);

                // 现在边界平滑分析已经完成,我们不再需要三角形列表。
                batcher.Pool.Return(ref Triangles);
                Inner.Flush(pairId, ref batcher);
                return true;
            }
            return false;
        }

    }
}
